#include "fly.h"
#include "pid.h"


extern PID_Struct Pitch_PID, Roll_PID,  Yaw_PID;
void FlyStop(void)
{
    TIM_SetCompare1(TIM3,2800);   
    TIM_SetCompare2(TIM3,2800);   
    TIM_SetCompare3(TIM3,2800);   
    TIM_SetCompare4(TIM3,2800);
}

void FlyAdvance(void)
{
    SetDesired(&Pitch_PID,2.f);
}   

void FlyBack(void)
{
    SetDesired(&Pitch_PID, -2.f);
}

void FlyLeft(void)
{
    SetDesired(&Roll_PID, 2.f);
}

void FlyRight(void)
{
    SetDesired(&Roll_PID, -2.f);
}

void SetFlyUp(float thrust)
{
    SetThrust(thrust);
}

float RotateValue = 0.f;
void SetRotate(u8 updown)
{
    if(updown == 1)
    {
        RotateValue += 0.5f;
    }
    else if(updown == 0)
    {
        RotateValue -= 0.5f;
    }
    SetDesired(&Yaw_PID, RotateValue);
}
    
